/**
 * @file test.cpp
 * @brief This file contains the implementation of inverse kinematics for a
 * robotic leg.
 */

// #include "params.h"
// #include "global.h"
#include <iostream>
#include <cmath>
#include <Eigen/Dense>

double H_X = 0.715; // (2.2)
double H_Y = 0.25;  // (2.2)

#define L1 0.05
#define L2 0.37
#define L3 0.40
#define MY_PI 3.1415926

#define TO_WORLD
// #define TODEGREE
// #define IKTEST
#define ZERO_OFFSET_SPOT

using Eigen::Vector3d;
using namespace std;

/**
 * Calculates the inverse kinematics for a given leg and foot position.
 *
 * @param legNumber The number of leg range from 0~3.
 * @param footPosition The position of the foot in Leg system.
 * @return  The joint angles required to achieve the given foot position.
 */
Vector3d inverseKinematics(int legNumber, Vector3d footPosition) {
  Vector3d motorAngles;

  double gamma, alfa, beta;

  double x = footPosition[0];
  double y = footPosition[1];
  double z = footPosition[2];

  double ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;

  gamma = MY_PI - acos(L1 / sqrt(pow(y, 2) + pow(z, 2))) - atan(-ksi * y / z);

  float L23 = sqrt(pow(x, 2) + pow(y, 2) + pow(z, 2) - pow(L1, 2));

  beta = acos((pow(L2, 2) + pow(L3, 2) - pow(L23, 2)) / (2 * L2 * L3));

  alfa = acos(x / L23) +
         acos((pow(L2, 2) + pow(L23, 2) - pow(L3, 2)) / (2 * L2 * L23));

#ifdef IK_TEST
  cout << "x: " << x << " y: " << y << " z: " << z << endl;
  cout << atan((L1 * cos(gamma) - z) / x) << endl;
  cout << asin(sqrt(pow(L1 * sin(gamma) - y, 2) + pow(L1 * cos(gamma) - z, 2)) /
               L23)
       << endl;

  cout << "gamma: " << gamma << endl;
  cout << "L23: " << L23 << endl;
  cout << "beta: " << beta << endl;
  cout << "alfa: " << alfa << endl;
#endif

#ifdef ZERO_OFFSET_SPOT
  gamma = (legNumber == 1 || legNumber == 3) ? gamma - MY_PI / 2
                                             : MY_PI / 2 - gamma;
  alfa = -alfa + (2 * MY_PI / 3);
  beta = -beta + 1.6;
#endif

#ifdef TO_DEGREE
  gamma = gamma * 180 / MY_PI;
  alfa = alfa * 180 / MY_PI;
  beta = beta * 180 / MY_PI;
#endif

  motorAngles[0] = gamma;
  motorAngles[1] = alfa;
  motorAngles[2] = beta;

  return motorAngles;
}

Vector3d legToBody(int legNumber, Vector3d footPosition) {
  switch (legNumber) {
  case 0: // RF
    return Vector3d(footPosition[0] + H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 1: // LF
    return Vector3d(footPosition[0] + H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 2: // RR
    return Vector3d(footPosition[0] - H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 3: // LR
    return Vector3d(footPosition[0] - H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  }
}

Vector3d bodyToLeg(int legNumber, Vector3d footPosition) {
  switch (legNumber) {
  case 0: // RF
    return Vector3d(footPosition[0] - H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 1: // LF
    return Vector3d(footPosition[0] - H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  case 2: // RR
    return Vector3d(footPosition[0] + H_X, footPosition[1] + H_Y,
                    footPosition[2]);
    break;
  case 3: // LR
    return Vector3d(footPosition[0] + H_X, footPosition[1] - H_Y,
                    footPosition[2]);
    break;
  }
}

/**
 * the forward kinematics for a given leg number and joint position.
 *
 * @param legNumber The number of leg range from 0~3.
 * @param jointPosition The angle of the joint motor: (gamma, alfa, beta).
 * @return The calculated foot position as a Vector3d object in Leg system.
 */
// Vector3d forwardKinematics(int legNumber, Vector3d jointPosition) {
//   double gamma = jointPosition[0];
//   double alfa = jointPosition[1];
//   double beta = jointPosition[2];

// #ifdef ZERO_OFFSET_SPOT
//   gamma = (legNumber == 1 || legNumber == 3) ? gamma + MY_PI / 2
//                                              : MY_PI / 2 - gamma;
//   alfa = -alfa + (2 * MY_PI / 3);
//   beta = -beta + 1.6;
// #endif

//   double delta = (legNumber == 0 || legNumber == 1) ? 1 : -1;
//   double ksi = (legNumber == 1 || legNumber == 3) ? 1 : -1;

//   double pow_L23 = pow(L2, 2) + pow(L3, 2) - 2 * L2 * L3 * cos(beta);

//   double x = L2 * cos(alfa) - L3 * cos(alfa + beta);
//   double y =
//       ksi * sqrt(pow_L23 - pow(x, 2)) * cos(gamma) + ksi * L1 * sin(gamma);
//   double z = -sqrt(pow_L23 - pow(x, 2)) * sin(gamma) + L1 * cos(gamma);

//   return Vector3d(x, y, z);
// }

Vector3d footTrajectory(Vector3d &start, Vector3d &end, double height,
                        double t_fai) {
  Vector3d result;
  result[0] =
      start[0] + (end[0] - start[0]) * (3 * pow(t_fai, 2) - 2 * pow(t_fai, 3));
  result[1] =
      start[1] + (end[1] - start[1]) * (3 * pow(t_fai, 2) - 2 * pow(t_fai, 3));

  double sigma_up = 2 * t_fai;       // 上升有效sigma
  double sigma_down = 2 * t_fai - 1; // 下降有效sigma
  double z_top = start[2] + height;  // 0.5时刻的高度

  if (sigma_up <= 1)
    result[2] =
        start[2] + height * (3 * pow(sigma_up, 2) - 2 * pow(sigma_up, 3));
  else if (sigma_down > 0)
    result[2] =
        z_top +
        (end[2] - z_top) * (3 * pow(sigma_down, 2) - 2 * pow(sigma_down, 3));

  return result;
}

int main() { return 0; }
